/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This class implements an Extended Kalman Filter that 
* estimates the Pose and Speed (and angular speed) of a robot.
* The model does not have any kinematic constraints, 
* it predicts the state based on speed and angular speed inputs.
* The prediction is based on given accelerations, so that if the 
* estimated speed or angular speed differs from the input, the accelerations
* are integrated into the states. 
* The update can be speed and angular speed measurement, or pose measurement 
*
* The class uses Newmat library (v10) for matrix manipulation. At least
* in debian/ubuntu the library can be installed using:
* sudo apt-get install libnewmat10-dev
* 
* To compile this class as standalone:
* g++ -o PoseSpeedEKF PoseSpeedEKF.cpp -DUSE_POSE_SPEED_EKF_MAIN -Duse_namespace -lnewmat
* 
**/
#ifndef _POSE_SPEED_EKF_H_
#define _POSE_SPEED_EKF_H_

#include<stdio.h>
#include <stdlib.h>
#include <math.h>

#ifndef COMPILE_LIB_FROM_SRC
		#include <newmat/newmat.h>
#else
#include "newmat.h"
#endif

#ifdef USE_POSE_SPEED_EKF_MAIN
		#include<ostream>
		#include<iostream>
		#include <newmat/newmatio.h> 
		#include<time.h>
#endif

using namespace NEWMAT; 

class CPoseSpeedEKF{
		public:
				/**
				* Constructor
				* The constructor requires the initial state (1st line), variances for
				* the initial state (2nd line), prediction (noise) variances (3rd line)
				* and measurement (noise) variances (4th line)
				* The acceleration parameters should be the ones that the real robot tries
				* to execute while performing speed control
				*/
				CPoseSpeedEKF(float posx, float posy,float posa, float v, float w,
											float varx, float vary, float vara, float varv, float varw,
											float qx, float qy, float qa, float qv, float qw,
											float rx, float ry, float ra, float rv, float rw,
										  float robot_acceleration, float robot_angular_acceleration);
				~CPoseSpeedEKF();
				
				/**
				 * Initialize the filter state values
				 **/
				void Initialize(float posx, float posy,float posa, float v, float w);
				
				/**
				* Predict from state k->k+1;
				* @param v Speed Control during the prediction
				* @param w Angular speed control during the prediction
				* @param dt time step in seconds 
				**/
				void predict(float v, float w, float dt);
				
				/**
				 * Predict from state k->k+1; Using differential movement from previous frame to this instance
				 *
				 * @param dl The length of the differential movement
				 * @param a  the direction of movement in differential frame [a = atan2((y(k+1)-y(k)),x(k+1)-x(k))]
				 * @param dPhii The change in heading  
				 **/
				void diffPosePredict(float dl, float a, float dPhii);
				void diffPosePredict(float dl, float a, float dPhii, float varx,float vary,float vara);
				/**
				* Updates the speed measurement 
				* @param v speed meassurement
				* @param w angular speed measurement
				**/
				void updateSpeedMeasurement(float v, float w);
				
				/**
				 * Updates the speed measurement 
				 * @param v speed meassurement
				 * @param w angular speed measurement
				 * @param rv variance of v
				 * @param rw variance of w 
				 **/
				void updateSpeedMeasurement(float v, float w, float rv, float rw);
				
				/**
				* Updates the pose measurement 
				* @param posx X-coordinate
				* @param posy Y-coordinate
				* @param posa Heading
				*/
				void updatePoseMeasurement(float posx, float posy, float posa);
				
				
				/**
				 * Updates the pose measurement 
				 * @param posx X-coordinate
				 * @param posy Y-coordinate
				 * @param posa Heading
				 * @param rx Variance of x
				 * @param ry Variance of y
				 * @param ra Variance of a
				 */
				void updatePoseMeasurement(float posx, float posy, float posa, float rx, float ry, float ra);
				
				
				void getState(float &posx, float &posy, float &posa, float &v, float &w);
				void getStateVariances(float &vx, float &vy, float &va, float &vv, float &vw);
				
				
		private:
				Matrix P;          ///< Covariance
				DiagonalMatrix Q;  ///< Prediction (noise) Variance
				DiagonalMatrix Rs;  ///< Speed Measurement (noise) Variance
				DiagonalMatrix Rp;  ///< Pose Measurement (noise) Variance
				Matrix Hs;          ///< Measurement Matrix for speed measurement
				Matrix Hp;          ///< Measurement Matrix for pose measurement
				ColumnVector x;    ///< State
				
				float acceleration;
				float angular_acceleration;
				
				/**
				* Calculate jacobian
				*/
				void df(Matrix &G, float dt);
				void diffPoseDf(Matrix &G, float dl, float a);
				void topi(double &angle);

};


#endif
